#ifndef _LIDAR_PROJECTION_H
#define _LIDAR_PROJECTION_H

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <Eigen/Dense>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <camera_projection.h>
#include "migratable_rl/detect.h"
#include <migratable_rl/semantics_lidar.h>
#include <visualization_msgs/Marker.h>



class LidarProjection{
public:
    LidarProjection(ros::NodeHandle& n);

private:
    void lidar_callback(const sensor_msgs::LaserScanConstPtr& msg);

    void image_callback(const sensor_msgs::ImageConstPtr& msg);

    void camerainfo_callback(const sensor_msgs::CameraInfoConstPtr& msg);



    Eigen::Matrix4d init_pose(std::vector<double> vector_pose);

    Eigen::Vector3d transform_point(double r,double theta);

    cv::Point3i projection_point(Eigen::Vector3d& point);

    void projection_point(std::vector<Eigen::Vector3d>& points,std::vector<cv::Point3i>& image_position);

    void visualize_point(cv::Mat& image);

    void distance_normal(std::vector<double>& distance);

    void visulize_semantics_lidar(migratable_rl::semantics_lidar& semantics_lidar);


private:
    ros::Subscriber lidar_sub;
    ros::Publisher semantics_lidar_pub;
    ros::Publisher semantics_lidar_visualize_pub;
    ros::ServiceClient detect_client;
    std::vector<ros::Subscriber> image_sub_vec;
    std::vector<ros::Subscriber> camerainfo_sub_vec;
    Eigen::Matrix4d lidar_pose_;

    std::vector<std::shared_ptr<CameraProjection>> camera_projection_ptr_vec_; 

};



#endif